#include<math.h>
#define DF float
#define NULL 0


//-------------------------------------------//
//矩阵减法
//-------------------------------------------//
/**/
void MatrixSub( float* fMatrixA,float* fMatrixB,float* Result,
		unsigned int m,unsigned int n )
{
	unsigned int index_i = 0;
	unsigned int index_j = 0;
	unsigned int itemp = 0;
	for (index_i=0;index_i<m;index_i++)
		for (index_j=0;index_j<n;index_j++)
		{
			itemp = index_i*n+index_j;
			*(Result+itemp) = *(fMatrixA+itemp) - *(fMatrixB+itemp);
		}
}
//void MatrixSub

//-------------------------------------------//
//矩阵乘法
//-------------------------------------------//
void MatrixMultiply( 	float* fMatrixA,unsigned int uRowA,unsigned int uColA,
			float* fMatrixB,unsigned int uRowB,unsigned int uColB,
			float* MatrixResult )
{
	unsigned int index_i = 0;
	unsigned int index_j = 0;
	unsigned int index_l = 0;
	unsigned int index_u = 0;
	unsigned int index_k = 0;
	unsigned int index_v = 0;
	uRowB = uRowB;
	for(index_i=0;index_i<uRowA;index_i++)
		for(index_j=0;index_j<uColB;index_j++)
		{
			index_u = index_i*uColB + index_j;
			MatrixResult[index_u] = 0.0;
			for(index_l=0;index_l<uColA;index_l++)
			{
				index_k = index_i*uColA+index_l;
				index_v = index_l*uColB+index_j;

				if ((((fMatrixA[index_k]))!=0.0) && (((fMatrixB[index_v]))!=0.0))
					(MatrixResult[index_u]) += ((fMatrixA[index_k])) * ((fMatrixB[index_v]));

			}
		 }
}

//-------------------------------------------//
//矩阵转置
//-------------------------------------------//
void MatrixTranspose(float* fMatrixA,unsigned int m,unsigned n,float* fMatrixB)
{
	unsigned int index_i = 0;
	unsigned int index_j = 0;
	unsigned int index_u = 0;
	unsigned int index_v = 0;

	for (index_i=0;index_i<m;index_i++)
		for (index_j=0;index_j<n;index_j++)
		{
			index_u = index_j*m+index_i;
			index_v = index_i*n+index_j;
			fMatrixB[index_u] = fMatrixA[index_v];
		}
}	//MatrixTranspose

//-------------------------------------------//
//矩阵单位矩阵生成
//-------------------------------------------//
/*
void MatrixE(float* fMatrixA,unsigned int n)
{
	unsigned int index_i = 0;
	unsigned int index_j = 0;

	for (;index_i<n;index_i++)
		for (index_j=0;index_j<n;index_j++)
		{
			if (index_i==index_j)
				*(fMatrixA+index_i*n+index_j) = 1;
			else
				*(fMatrixA+index_i*n+index_j) = 0;
		}
}	*///MatrixE


//求矩阵行列式的值
//全选主元高斯消去法
//a-长度为n*n的数组
//n-矩阵的阶数
void dhdet(float *a,int n,float det)
{
int i,j,k,is,js,l,u,v;
    float f,q,d;
    f=1.0; det=1.0;
    for (k=0; k<=n-2; k++)
{
  q=0.0;
        for (i=k; i<=n-1; i++)
   for (j=k; j<=n-1; j++)
   {
    l=i*n+j; d=fabs(a[l]);
    if (d>q)
    {
     q=d;
     is=i;
     js=j;
    }
   }
   if (q+1.0==1.0)
   {
    det=0.0;

   }
   if (is!=k)
   {
    f=-f;
    for (j=k; j<=n-1; j++)
    {
     u=k*n+j; v=is*n+j;
     d=a[u]; a[u]=a[v]; a[v]=d;
    }
   }
   if (js!=k)
   {
    f=-f;
    for (i=k; i<=n-1; i++)
    {
     u=i*n+js; v=i*n+k;
     d=a[u]; a[u]=a[v]; a[v]=d;
    }
   }
   l=k*n+k;
   det=det*a[l];
   for (i=k+1; i<=n-1; i++)
   {
    d=a[i*n+k]/a[l];
    for (j=k+1; j<=n-1; j++)
    {
     u=i*n+j;
     a[u]=a[u]-d*a[k*n+j];
    }
   }
}
    det=f*det*a[n*n-1];

}
//-------------------------------------------//
//MatrixDet2
//二姐矩阵行列式的值
//-------------------------------------------//
double MatrixDet2(float* fMatrixA)
{
	return (*fMatrixA)*(*(fMatrixA+3))-(*(fMatrixA+1))*(*(fMatrixA+2));
}	//MatrixDet2


//-------------------------------------------//
//矩阵求逆
//-------------------------------------------//
int MatrixInverse(float* fMatrixA,int n,float ep)
{
	int i = 0;
	int j = 0;
	int k = 0;
	int l = 0;
	int u = 0;
	int v = 0;
	int temp_row[10],temp_col[10];
	int * prow = temp_row,urow = 0;		//?????????
	int * pcol = temp_col,ucol = 0;			//?????????
	float ftemp_max = 0.0;
	float ftemp = 0.0;

	ep = ep;					//??ep???????

	for (i=0;i<10;i++)
	{
		temp_row[i] = 0;
		temp_col[i] = 0;
	}

	for (k=0;k<n;k++)
	{
		ftemp_max =0.0;		//????????????
		for (i=k;i<n;i++)
			for (j=k;j<n;j++)
			{
				l = i*n+j;
				ftemp = fabs(*(fMatrixA+l));
				if (ftemp_max<ftemp)
				{
					ftemp_max = ftemp;
					*(prow+k) = urow = i;
					*(pcol+k) = ucol= j;
				}
			}


		if (urow!=k)			//?????????????
		{
			for (j=0;j<n;j++)
			{
				u = k*n +j;
				v = urow*n+j;
				ftemp = *(fMatrixA+u);
				*(fMatrixA+u) = *(fMatrixA+v);
				*(fMatrixA+v) = ftemp;
			}
		}

		if (ucol!=k)			//?????????????
		{
			for (i=0;i<n;i++)
			{
				u = i*n+k;
				v = i*n+ucol;
				ftemp = *(fMatrixA+u);
				*(fMatrixA+u) = *(fMatrixA+v);
				*(fMatrixA+v) = ftemp;
			}
		}

		l = k*n+k;
		ftemp = *(fMatrixA+l) = 1.0/(*(fMatrixA+l));

		for (j=0;j<n;j++)
		{
			if (j!=k)
			{
				u = k*n+j;
				*(fMatrixA+u) *= *(fMatrixA+l);
			}
		}

		for (i=0;i<n;i++)			//???????
		{
			if (i!=k)
			{
				for (j=0;j<n;j++)
				{
					if (j!=k)
					{
						u = i*n+j;
						*(fMatrixA+u) -= (*(fMatrixA+i*n+k))*(*(fMatrixA+k*n+j));
					}
				}
			}
		}

		for (i=0;i<n;i++)
		{
			if (i!=k)
			{
				u = i*n+k;
				*(fMatrixA+u) = -(*(fMatrixA+u))*(*(fMatrixA+l));
			}
		}
	}

	for (k=n-1;k>=0;k--)
	{
		if ((*(pcol+k))!=k)
		{
			for (j=0;j<n;j++)
			{
				u = k*n+j;
				v = (*(pcol+k))*n+j;
				ftemp = *(fMatrixA+u);
				*(fMatrixA+u) = *(fMatrixA+v);
				*(fMatrixA+v) = ftemp;
			}
		}

		if ((*(prow+k))!=k)
		{
			for (i=0;i<n;i++)
			{
				u = i*n+k;
				v = i*n+(*(prow+k));
				ftemp = *(fMatrixA+u);
				*(fMatrixA+u) = *(fMatrixA+v);
				*(fMatrixA+v) = ftemp;
			}
		}
	}
	return 1;
}	//MatrixInverse2

//-------------------------------------------//
//求矩阵范数
//-------------------------------------------//
/*
float Norm(float *fMatrixA,int iRow,int iCol)
{
	int i = 0;
	int j = 0;
	int index = 0;
	float local_result = 0.0;

	for (i=0;i<iRow;i++)
		for (j=0;j<iCol;j++)
		{
			index = i * iCol +j;
			local_result += (*(fMatrixA+index)) * (*(fMatrixA+index));
		}

	local_result = sqrt(local_result);

	return local_result;
}*/



void UD(float * A,int n,float * U,float * D)
{
	int i,j,k;
	float sum1=0;
	float sum2=0;
	int m=n-1;

	for(i=0;i<n;i++)
	{
		for(j=0;j<n;j++)
		{
			if(i==j)
			{
				U[i*n+j]=1;
				D[i*n+j]=0;
			}
			else
			{
				D[i*n+j]=0;
				U[i*n+j]=0;
			}
		}
	}


	D[m*n+m]=A[m*n+m];


	for(i=0;i<n;i++)
	{
		U[i*n+m]=A[i*n+m]/D[m*n+m];
	}


	for(j=n-2;j>=0;j--)
	{

		for(k=j+1;k<n;k++)
		{
			sum2=sum2+D[k*n+k]*U[j*n+k]*U[j*n+k];
		}

		D[j*n+j]=A[j*n+j]-sum2;
		sum2=0;

		for(i=0;i<j;i++)
		{
			for(k=j+1;k<n;k++)
			{
			   	sum1=sum1+D[k*n+k]*U[i*n+k]*U[j*n+k];
			}

			U[i*n+j]=(A[i*n+j]-sum1)/D[j*n+j];
			sum1=0;
		}


	}

}

